'''
Mission 7, Final code solution
'''
from botcore import *
from time import sleep

power = 1
thresh = 75
go_motors = False
SPEED = 20
n_sample = 0
target = 0

def cal_thresh():
    global thresh
    det = 101
    sensed = prox.range(10, power)
    if sensed[LEFT] > 0:
        det = sensed[LEFT]
    if sensed[RIGHT] > 0:
        det = min(det, sensed[RIGHT])
    if det > 100:
        thresh = 100
    else:
        thresh = det - 5
    print("Sensed=", sensed, ", set thresh=", thresh)

def cal_power():
    global power
    power = 0
    while power < 8:
        power = power + 1
        cal_thresh()
        if thresh < 100:
            break

def drive(speed, turn_ratio):
    turn_spd = speed * turn_ratio
    fwd_spd = speed - abs(turn_spd)
    motors.run(LEFT, fwd_spd + turn_spd)
    motors.run(RIGHT, fwd_spd - turn_spd)    

# -- Main Program --

while True:
    if buttons.was_pressed(1):
        cal_power()
        print("CAL: Power=", power, ", thresh=", thresh)

    if buttons.was_pressed(0):
        go_motors = not go_motors
        motors.enable(go_motors)
        leds.user_num(0, go_motors)

    p = prox.detect(power, thresh)
    leds.prox(p)

    n_sample = n_sample + 1

    if p[LEFT]:
        target = target - 1
    if p[RIGHT]:
        target = target + 1

    if n_sample == 100:
        drive(SPEED, target / n_sample)
        n_sample = 0
        target = 0